#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from geometry_msgs.msg import PoseStamped
from mavros_msgs.srv import SetMode, CommandBool
import math
import time

# 经纬度转局部坐标系的函数
def lat_lon_to_enu(start_lat, start_lon, target_lat, target_lon):
    R = 6378137.0  # 地球半径（米）
    start_lat_rad = math.radians(start_lat)
    start_lon_rad = math.radians(start_lon)
    target_lat_rad = math.radians(target_lat)
    target_lon_rad = math.radians(target_lon)

    d_lat = target_lat_rad - start_lat_rad
    d_lon = target_lon_rad - start_lon_rad

    east = R * d_lon * math.cos((start_lat_rad + target_lat_rad) / 2)
    north = R * d_lat

    return east, north

def takeoff_and_land(start_lat, start_lon, target_lat, target_lon, target_alt=3.0, speed=2.0):
    rospy.init_node('drone_flight', anonymous=True)
    rospy.wait_for_service('/mavros/set_mode')
    rospy.wait_for_service('/mavros/cmd/arming')

    try:
        set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
        arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)

        arming(True)
        rospy.loginfo("Drone armed.")
        set_mode(0, "OFFBOARD")
        rospy.loginfo("Offboard mode set.")

        takeoff_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
        pose = PoseStamped()
        pose.header.frame_id = "map"
        pose.pose.position.z = target_alt

        for _ in range(100):  # 发布起飞指令
            takeoff_pub.publish(pose)
            time.sleep(0.1)

        rospy.loginfo("Taking off to %f meters.", target_alt)
        time.sleep(10)  # 等待无人机上升

        # 将目标经纬度转换为局部坐标
        east, north = lat_lon_to_enu(start_lat, start_lon, target_lat, target_lon)

        target_pose = PoseStamped()
        target_pose.header.frame_id = "map"
        target_pose.pose.position.x = east
        target_pose.pose.position.y = north
        target_pose.pose.position.z = target_alt

        distance = math.sqrt(east**2 + north**2)
        flight_time = distance / speed

        start_time = rospy.get_time()
        while rospy.get_time() - start_time < flight_time:
            takeoff_pub.publish(target_pose)
            time.sleep(0.1)

        rospy.loginfo("Flying to target position (%f, %f).", target_lat, target_lon)
        time.sleep(5)  # 等待一段时间

        # 降落
        landing_pub = rospy.Publisher('/mavros/cmd/land', CommandBool, queue_size=10)
        landing_pub.publish(True)
        rospy.loginfo("Landing at target position.")

    except rospy.ServiceException as e:
        rospy.logerr("Service call failed: %s" % e)

if __name__ == '__main__':
    start_latitude = 30.0  # 起始纬度
    start_longitude = 120.0  # 起始经度
    target_latitude = 30.001  # 目标纬度
    target_longitude = 120.001  # 目标经度
    takeoff_and_land(start_latitude, start_longitude, target_latitude, target_longitude)

